#include "forco_os.h"

//系统数字越小优先级越高
//申明一个数组用于标识别优先级,根据位数来标示一个bit标示这个优先级上是否有任务
CPU_UINT32  OSPrioTbl[OS_PRIO_TBL_SIZE];
//高字节优先级低为

//初始化优先级标,每一位都清0
void OS_PrioInit(void)
{
  CPU_UINT32 priocount = 0;
  for(priocount=0;priocount<OS_PRIO_TBL_SIZE;priocount++){
    OSPrioTbl[priocount]    = 0;
  }
}

//设置任务的优先级
void OS_PrioInsert(CPU_UINT32   prio)
{
    CPU_UINT32  bit;
    CPU_UINT32  bit_nbr;
    CPU_UINT32  prioix;

    //计算应该插入在数组的那个元素中
    prioix  =   prio/OSCfg_CPU_BITS;
    //计算要设置的位
    bit_nbr =   prio&(OSCfg_CPU_BITS-1);
    //设置优先级位
    bit     = 1<<((OSCfg_CPU_BITS -1)-bit_nbr);
    //置位数组中的元素项
    OSPrioTbl[prioix]   |= bit;
}

//清除优先级位
void OS_PrioRemove(CPU_UINT32  prio)
{

    CPU_UINT32  bit;
    CPU_UINT32  bit_nbr;
    CPU_UINT32  prioix;

    //计算应该插入在数组的那个元素中
    prioix  =   prio/OSCfg_CPU_BITS;
    //计算要设置的位
    bit_nbr =   prio&(OSCfg_CPU_BITS-1);
    //设置优先级位
    bit     = 1<<((OSCfg_CPU_BITS -1)-bit_nbr);
    //置位数组中的元素项
    OSPrioTbl[prioix]   &= ~bit;
}

//查找最高优先级
CPU_UINT32 OS_PrioGetHighest(void)
{
   CPU_UINT32   *p_prio;
   CPU_UINT32   prio;

   prio     =   0;
   //获取优先级根节点
   p_prio   = &OSPrioTbl[0];
   //找非0的元素
   while(*p_prio == 0){
        prio    +=  OSCfg_CPU_BITS;
        p_prio++;
   }
   //根据前导0返回优先级
   prio         += CPU_CntLeadZeros(*p_prio);
   return prio;  
}









